#include "robot_manager.h"

void RobotManager::commandMoveJointCallback(const std_msgs::Float64MultiArray::ConstPtr &msg)
{
    if (msg->data.size() == 7)
    {
        ROS_INFO_STREAM("[Move Joint] Received command: "
                        << "Joint1: " << msg->data[0] << ", "
                        << "Joint2: " << msg->data[1] << ", "
                        << "Joint3: " << msg->data[2] << ", "
                        << "Joint4: " << msg->data[3] << ", "
                        << "Joint5: " << msg->data[4] << ", "
                        << "Joint6: " << msg->data[5] << ", "
                        << "Joint7: " << msg->data[6]);
    }
    else
    {
        ROS_WARN("[Move Joint] Received command does not contain 7 values.");
    }

    std::vector<double> mit_kp, mit_kd;
    nh_.getParam("mit_kp", mit_kp);
    nh_.getParam("mit_kd", mit_kd);
    std::vector<double> D = {1.0, 1.2, 1.1, 1.1, 1.3, 1.3, 1.0};

    ZORAL::PlannerProperties properties;
    properties.traj_solve_type = ZORAL::TrajSolveType::FIVE_POLY_INTERP;
    properties.min_move_time = 5;
    properties.sample_rate = CONTROL_FREQ;
    ZORAL::Trajectory traj;
    std::vector<std::vector<double>> start_state(1), end_state(1);
    pthread_mutex_lock(&zoros_mutex_);
    start_state[0] = joint_state_msg_.position;
    pthread_mutex_unlock(&zoros_mutex_);

    end_state[0] = msg->data;

    int ret = robot_plan_->move(ZORAL::PlannerType::MOVE_JOINT, properties, start_state, end_state, traj);
    std::vector<std::vector<double>> traj_pos = traj.getPos();
    std::vector<std::vector<double>> traj_vel = traj.getVel();

    for (int k = 0; k < traj_pos.size(); k++)
    {
        ROS_INFO_STREAM(traj_pos[k][0] << ", " << traj_pos[k][1] << ", " << traj_pos[k][2] << ", " << traj_pos[k][3]
                                        << ", " << traj_pos[k][4] << ", " << traj_pos[k][5] << ", " << traj_pos[k][6]
                                        << ", ");
    }
    ROS_INFO("plan ret: %d, traj_pos_size: %ld", ret, traj_pos.size());

    ros::Rate rate(CONTROL_FREQ);
    int k = 0;
    std::vector<double> q(7, 0.0), qd(7, 0.0), qdd(7, 0.0), torque;
    while (1)
    {
        pthread_mutex_lock(&zoros_mutex_);
        std::vector<double> actual_position = joint_state_msg_.position;
        pthread_mutex_unlock(&zoros_mutex_);

        if (fabs(actual_position[0] - msg->data[0]) < 1e-3 && fabs(actual_position[1] - msg->data[1]) < 1e-3 &&
            fabs(actual_position[2] - msg->data[2]) < 1e-3 && fabs(actual_position[3] - msg->data[3]) < 1e-3 &&
            fabs(actual_position[4] - msg->data[4]) < 1e-3 && fabs(actual_position[5] - msg->data[5]) < 1e-3 &&
            fabs(actual_position[6] - msg->data[6]) < 1e-3)
        {
            break;
        }

        if (k >= traj_pos.size())
        {
            break;
        }

        pthread_mutex_lock(&zoros_mutex_);
        q = joint_state_msg_.position;
        robot_model_->inverseDynamics(q, qd, qdd, torque);
        for (size_t i = 0; i < 7; i++)
        {
            if (!is_simulation_)
            {
                //! 实机模式下还需调用 DM SDK 获取实际的关节角度
                // dm_->control_pos_vel(motors_[i], traj_pos[k][i], 10.0);
                dm_->control_mit(motors_[i], mit_kp[i], mit_kd[i], traj_pos[k][i], traj_vel[k][i], torque[i] * D[i]);
            }
            else
            {
                joint_state_msg_.position[i] = traj_pos[k][i];
                //! 如果是仿真模式，发布一条初始消息，所有关节的位置为0.0
                joint_state_msg_.header.stamp = ros::Time::now();
                joint_pub_.publish(joint_state_msg_); // 发布初始的 joint_states 消息
            }
        }
        pthread_mutex_unlock(&zoros_mutex_);

        k++;
        rate.sleep();
    }
}